/**
* @file registration.h
* @author Alberto Basso, Stefano Squizzato, Maroso Alessandro, Luca Tumelero
* @brief Set of methods for registrate a source cloud to a target cloud
*/

#ifndef REGISTRATION_H
#define REGISTRATION_H

#include <pcl/io/pcd_io.h>
#include <pcl/registration/correspondence_types.h>
#include <pcl/search/kdtree.h>

using namespace std;
using namespace pcl;


/**
 * @brief Register source cloud on target cloud using FPFH and uniform keypoints
 *
 * @param[in] sourceColor Pointer to source cloud
 * @param[in] targetColor Pointer to target cloud
 * @param[out] registeredColor Pointer to registered source cloud
 */
void registerSourceToTarget (
        PointCloud<PointXYZRGB>::Ptr sourceColor ,
        PointCloud<PointXYZRGB>::Ptr targetColor,
        PointCloud<PointXYZRGB>::Ptr registeredColor);


/**
 * @brief Calculate matrix transformation and register source cloud on target one using ICP algorithm
 *
 * @param[in] source Pointer to source cloud
 * @param[in] target Pointer to target cloud
 * @param[out] registered Pointer to registered source cloud
 * @param[out] final_transformation_matrix Final transformation matrix
 * @param[in] resolution leaf size for calculation
 */
void finalRegisterSourceToTarget (
        PointCloud<PointXYZ>::Ptr source ,
        PointCloud<PointXYZ>::Ptr target,
        PointCloud<PointXYZ>::Ptr registered,
        Eigen::Matrix4f &final_transformation_matrix,
        float resolution);

template< class PointT >
/**
 * @brief Calculate score of proximity for point in source and target based on distances
 * @param[in] source Source pointCloud
 * @param[in] target Target pointCloud
 * @return float Score
 */
float validationScoreProximityTarget (PointCloud<PointT> &source, PointCloud<PointT> &target)
{
    boost::shared_ptr< PointCloud<PointT> > sourcePtr = source.makeShared();
    search::KdTree<PointT> tree;
    tree.setInputCloud(sourcePtr);
    float sum=0;
    for (int i=0;i<target.size();i++)
    {
        PointT currentPoint=target.at(i);
        vector<int> indices;
        vector<float> sqrDist;
        tree.nearestKSearch(currentPoint, 1, indices, sqrDist);
        sum+=sqrDist.at(0);
    }
    return sum/target.size();
}

#endif
